h = 0.1;                        % Define step to increment by
t = 0:h:100;                    % Define time to increment step; upto maximum value

omega = zeros(1, length(t));    % Create an empty array for omega
omega(1) = 0.25;                % Initialise the first index of the array to zero

theta = zeros(1, length(t));    % Create an empty array for theta
theta(1) = 0;                   % Initialise the first index of the array to zero

x = zeros(1, length(t));        % Create an empty array for X
x(1) = 0;                       % Initialise the first index of the array to zero

y = zeros(1, length(t));        % Create an empty array for Y
y(1) = 0;                       % Initialise the first index of the array to zero

z = zeros(1, length(t));        % Create an empty array for Z
z(1) = 0;                       % Initialise the first index of the array to zero


for i = 1: (length(t)-1);       % Loop through array
       
    % Call the ode function passing in the iteration of omega and theta    
    alpha = ode(omega(i), theta(i));
    
    % Calculate omega and theta values
    omega(i+1) = omega(i) + alpha * h;
    theta(i+1) = theta(i) + omega(i+1) * h;
    
    % Calculate x, y and z values
    x(i) = 100 * cos(theta(i));
    y(i) = 100 * sin(theta(i));
    z(i) = 10 * 0.5 * cos(4*theta(i));
    
end

close all;
figure;plot(theta, t);title('Angular Simulation');xlabel('Theta');ylabel('Time');                                   % Car Angle over time
figure;plot(omega, t);title('Angular Velocity');xlabel('Omega');ylabel('Time');                                     % Car Velocity over time
figure;plot(alpha, t);title('Angular Acceleration');xlabel('Alpha');ylabel('Time');                                 % Car Acceleration over time
figure;plot3(x, y, z);title('Car Position in 3D Space');xlabel('X Position');ylabel('Y Position');zlabel('Z Position');% Car Position in 3D space
figure;plot(x, y);title('Car Position in 2D Space');xlabel('X Position');ylabel('Y Position');                      % Car Position in 2D space
figure;plot(x, t);title('X Position over time');xlabel('X Position');ylabel('Time');                                % Car X Position over time
figure;plot(y, t);title('Y Position over time');xlabel('Y Position');ylabel('Time');                                % Car Y Position over time
figure;plot(z, t);title('Z Position over time');xlabel('Z Position');ylabel('Time');                                % Car Z Position over time
figure;plot3(x, y, theta);title('Car X and Y position over Angle');                                                 % Car X and Y Positions over Angle
figure;plot3(x, theta, t);title('Car X orientation over time');xlabel('X Positiion');ylabel('Theta');zlabel('Time');% Car X Position and Angle over time
figure;plot3(y, theta, t);title('Car Y orientation over time');xlabel('Y Positiion');ylabel('Theta');zlabel('Time');% Car Y Position and Angle over time
figure;plot3(z, theta, t);title('Car Z orientation over time');xlabel('Z Positiion');ylabel('Theta');zlabel('Time');% Car Z Position and Angle over time